How to calculate the magnitude of a 3D vector